#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_srvs/srv/set_bool.hpp"

#include "water_tank/water_tank.hpp"

class WaterTankSimNode : public rclcpp::Node {
public:
  WaterTankSimNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "Node %s has been started.", name.c_str());

    // 初始化水箱水位话题发布者
    water_level_publisher_ =
        this->create_publisher<std_msgs::msg::Float32>("water_level", 10);

    // 初始化水箱进水服务
    water_in_service_ = this->create_service<std_srvs::srv::SetBool>(
        "set_water_in",
        [&](const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
            std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
          handleSetWaterIn(request, response);
        });

    // 创建定时器，定期更新水位并发布
    timer_ = this->create_wall_timer(
        std::chrono::duration<float>(1.0 / publish_rate_),
        [&]() { timerCallback(); });
  }

private:
  // 发布频率，单位Hz
  const float publish_rate_ = 1.0;
  // 水箱对象
  WaterTank water_tank_;
  // 水箱水位发布者
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr water_level_publisher_;
  // 定时器
  rclcpp::TimerBase::SharedPtr timer_;
  // 水箱进水服务
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr water_in_service_;

  // 定时器回调函数，更新水位并发布
  void timerCallback() {
    water_tank_.updateWaterLevel();
    auto message = std_msgs::msg::Float32();
    message.data = water_tank_.getWaterLevel();
    water_level_publisher_->publish(message);
    RCLCPP_DEBUG(this->get_logger(), "Water level: %.2f", water_tank_.getWaterLevel());
  }

  // 水箱进水服务回调函数
  void handleSetWaterIn(
      const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
      std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
    if (request->data) {
      water_tank_.setWaterTankState(WaterTankState::UP);
      response->success = true;
      response->message = "Water tank is filling up.";
      RCLCPP_INFO(this->get_logger(), "Fill up.");
    } else {
      water_tank_.setWaterTankState(WaterTankState::DOWN);
      response->success = true;
      response->message = "Water tank is draining.";
      RCLCPP_INFO(this->get_logger(), "Drain.");
    }
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<WaterTankSimNode>("water_tank_sim_node");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}